# _*_ coding: utf-8 _*_
import time
import datetime
import struct

import opQue
import locProtocol

class Timer:
    def __init__(self):
        self.tm_year = int  # 年
        self.tm_mon = int   # 月
        self.tm_mday = int  # 日
        self.tm_hour = int  # 时
        self.tm_min = int   # 分
        self.tm_sec = int   # 秒
        pass


class GpsData:
    def __init__(self):
        self.latitude = float
        self.longitude = float
        self.speed = int
        self.direction = int
        self.alive = True
        self.sec = float
        pass
    timer = Timer
    pass

    class MpuData:
        def __init__(self):
            self.ax = float
            self.ay = float
            self.az = float
            self.gx = float
            self.gy = float
            self.gz = float
            pass
        pass
    pass



# """ 找到GPS或者mpu数据开头 """
def find_cmd_start(str, len):
    # str = list(str)
    cmd_flag = 0
    for i in range(len - 6):
        if str[i] == '$' and str[i+1] == 'G' and  str[i+2] == 'N' and str[i+3] == 'R' \
                and str[i+4] == 'M' and str[i+5] == 'C':
            cmd_flag = 1    # 1表示有GNRMC数据
            return i,cmd_flag
        # 下面部分是关于gngaa解析的，暂时用不到，用到再添加
        # elif str[i] == '$' and str[i+1] == 'G' and  str[i+2] == 'N' and str[i+3] == 'G' \
        #         and str[i+4] == 'G' and str[i+5] == 'A':
        #     cmd_flag = 2
        #     return i,cmd_flag
        elif str[i] == 0x7E and str[i+1] == 0x7F and str[i+2] == 0x0C:   # mpu6050数据解析
            cmd_flag = 3 # 3表示有mpu6050数据
            return i, cmd_flag
        else:
            cmd_flag = 0
    return -1, cmd_flag


# """ 解析GPS和mpu6050数组 """
def parse_packet(GpsData, data, len):
    start_pos, cmd_flag = find_cmd_start(data, len)
    # print(cmd_flag)
    if start_pos == -1:
        return False, -1
    if cmd_flag == 1:
        end_pos = start_pos
        for i in range(start_pos,len - 1):
            if data[i] == '\r' and data[i+1] == '\n':
                end_pos = i + 1
                break
        if start_pos == end_pos:
            return False, -1
        else:
            # data[end_pos] = '\0'
            signal_flag = GPS_analysis(GpsData, data[start_pos:end_pos])
            if signal_flag:
                return True, cmd_flag
            else:
                return False, -1
            pass
        pass
    elif cmd_flag == 3: # 如果存在mpu6050数据
        # 找到数据头尾，若头等于尾则数据无效
        end_pos = start_pos
        for i in range(start_pos,len - 1):
            if data[i] == '\r' and data[i+1] == '\n':
                end_pos = i + 1
                break
        if start_pos == end_pos:
            return False, -1
        else:  # 数据有效，解析mpu6050
            signal_flag = MPU_analysis(GpsData, data[start_pos:end_pos])
            if signal_flag:
                return True, cmd_flag
            else:
                return False, -1
            pass

        pass
    else:
        return False, -1
    # 下面部分是关于gngaa解析的，暂时用不到，用到再添加
    # elif cmd_flag == 2:
    #     end_pos = start_pos
    #     for i in range(len - 1):
    #         if data[i] == '\r' and data[i+1] == '\n':
    #             end_pos = i + 1
    #             break
    #
    #     if start_pos == end_pos:
    #         return -1
    #     else:
    #         return end_pos + 1


# GNRMC解析
def GPS_analysis(GpsData, gps_buffer):
    try:
        temp = gps_buffer.split(',')
    except Exception as ex:
        print(str(ex))

    if temp[0] is not None and temp[0].find('RMC') != -1:
        if temp[2].find('V') != -1:
            GpsData.alive = False
            # print(u"GPS信号无效")
            return False
        elif temp[2].find('A') != -1:
            # Data_H = 0
            # Data_L = 0
            GpsData.alive = True

            if temp[1] != '':  # 时分秒报文信息
                Data_L = int(float(temp[1]))

            if temp[3] != '':
                latitude = float(temp[3])
                temp1 = latitude / 100
                temp2 = int(temp1)
                GpsData.latitude = temp2 + (temp1 - temp2) * 100 / 60  # 地图纠偏

            if temp[5] != '':
                longitude = float(temp[5])
                temp1 = longitude / 100
                temp2 = int(temp1)
                GpsData.longitude = temp2 + (temp1 - temp2) * 100 / 60  # 地图纠偏

            if temp[7] != '':
                Spd = float(temp[7])
                GpsData.speed = int(Spd * 100)

            if temp[8] != '':
                Cog = float(temp[8])
                GpsData.direction = int(Cog * 100)
            else:
                GpsData.direction = 0

            if temp[9] != '':
                Data_H = int(temp[9])
                GpsData.timer.tm_year = (Data_H % 100) + 2000   # 年
                GpsData.timer.tm_mon = int((Data_H % 10000) / 100)  # 月
                GpsData.timer.tm_mday = Data_H // 10000  # 日
                GpsData.timer.tm_hour = (Data_L // 10000)   # 时
                GpsData.timer.tm_min = int((Data_L % 10000) / 100)  # 分
                GpsData.timer.tm_sec = Data_L % 100     # 秒
                # print('Data_H: %s' % Data_H)
                # print('Data_L: %s' % Data_L)
                # print(GpsData.timer.tm_year, GpsData.timer.tm_mon,
                # GpsData.timer.tm_mday, GpsData.timer.tm_hour, GpsData.timer.tm_min,
                #       GpsData.timer.tm_sec)
                dateC1 = datetime.datetime(
                GpsData.timer.tm_year, GpsData.timer.tm_mon, GpsData.timer.tm_mday,
                GpsData.timer.tm_hour, GpsData.timer.tm_min, GpsData.timer.tm_sec)
                GpsData.sec = time.mktime(dateC1.timetuple())
                # print(GpsData.sec)
                pass
            return True
            pass
        else:
            return False
        pass


def MPU_analysis(GpsData,data):
    # 解析加速度数据，单位g(/ 9.8m/s^2)：
    GpsData.MpuData.ax = struct.unpack('!h',struct.pack('!H',((data[3]<<8) | data[4])))[0] / 2048  # x轴加速度
    GpsData.MpuData.ay = struct.unpack('!h',struct.pack('!H',((data[5]<<8) | data[6])))[0] / 2048  # y轴加速度
    GpsData.MpuData.az = struct.unpack('!h',struct.pack('!H',((data[7]<<8) | data[8])))[0] / 2048 # z轴加速度
    # 解析角速度,度/s：
    GpsData.MpuData.gx = struct.unpack('!h',struct.pack('!H',((data[9]<<8) | data[10])))[0] / 131    # x轴角速度
    GpsData.MpuData.gy = struct.unpack('!h',struct.pack('!H',((data[11]<<8) | data[12])))[0] / 131   # y轴角速度
    GpsData.MpuData.gz = struct.unpack('!h',struct.pack('!H',((data[13]<<8) | data[14])))[0] / 131 # z轴角速度

    print(GpsData.MpuData.ax, GpsData.MpuData.ay, GpsData.MpuData.az,
    GpsData.MpuData.gx, GpsData.MpuData.gy, GpsData.MpuData.gz)

    return True


def gpsAnalysis(queue1, queue2):
    # 将GPS数据写入文本
    str = './data/gpsdata/gpsdata ' + datetime.datetime.now().strftime("%Y-%m-%d %H-%M-%S") + '.txt'
    f = open(str, 'a+')
    while True:
        temp_data = opQue.readQ(queue1) # 从队列1中读取，打包完后发向队列2
        f.write(temp_data)  # 写入数据

        gps_data = GpsData
        if temp_data is not None:
            flag, cmd_flag = parse_packet(gps_data, temp_data, len(temp_data))
            if flag is True:
                if gps_data is not None and cmd_flag == 1:
                    # gps信号打包,得到流信号data
                    data = locProtocol.DCP_TX0x93Packet(gps_data)
                    # 将data写入队列中
                    opQue.writerQ(queue=queue2, data=data)
                elif gps_data is not None and cmd_flag == 3:
                    print('receive mpu6050 data')
                else:
                    print('empty data')
            else:
                print('data error, no analysis')
        else:
            time.sleep(0.1)
    f.close()


if __name__ == '__main__':
    # GPS测试数据
    # gps = GpsData
    # gps_buffer = '$GNRMC,092927.000,A,2235.9058,N,11400.0518,E,0.000,74.11,151216,,D*49\r\n'
    # #GPS_analysis(gps,gps_buffer)
    # #print(len(gps_buffer))
    # parse_packet(gps,gps_buffer,len(gps_buffer))
    # print(gps.latitude, gps.longitude, gps.speed, gps.direction)
    # #print(gps.timer.year,gps.timer.month,gps.timer.day,gps.timer.hour,gps.timer.minute,gps.timer.second,sep='-')

    # 惯导测试数据
    gps = GpsData
    a = b'\x7E\x7F\x0C\xFE\xDC\x00\x07\x08\x3B\xFE\x88\x00\x17\x00\x66\x0D\x0A'
    print(len(a))
    print(a[0])
    MPU_analysis(gps, a)
    print(gps.MpuData.ax)
